#include "Robot.h"

/*friend bool operator ==(Point p1,Point p2)
{
}*/

Robot::Robot(void)
{
}


Robot::~Robot(void)
{
}
void Robot::forward()
{
	turnCost -= 1;
	switch(direction)
	{
	case LEFT:
		X -= 1;
		break;
	case DOWN:
		Y += 1;
		break;
	case RIGHT:
		X += 1;
		break;
	case UP:
		Y -= 1;
		break;
	}
	char buffer[10];
	string xStr(_itoa(X,buffer,10));
	string yStr(_itoa(Y,buffer,10));
	listAction.push_back("FW("+ yStr +","+ xStr +")");
}
int Robot::toTheLeft(int direction)
{
	switch(direction)
	{
	case LEFT:
		return DOWN;
	case DOWN:
		return RIGHT;
	case RIGHT:
		return UP;
	case UP:
		return LEFT;
	}
	return -1;
}
int Robot::toTheRight(int direction)
{
	switch(direction)
	{
	case LEFT:
		return UP;
	case DOWN:
		return LEFT;
	case RIGHT:
		return DOWN;
	case UP:
		return RIGHT;
	}
	return -1;
}
void Robot::turnLeft()
{
	turnCost -= 1;
	direction = toTheLeft(direction);					
	listAction.push_back("TL");

}
void Robot::turnRight()
{
	turnCost -= 1;
	direction = toTheRight(direction);
	listAction.push_back("TR");
}
bool Robot::shoot()
{
	if(nArowLeft>0)
	{
		turnCost -= 10;
		nArowLeft -= 1;
		char buffer[10];
		string xStr(_itoa(mummyX,buffer,10));
		string yStr(_itoa(mummyY,buffer,10));
		listAction.push_back("SHOOT("+ yStr +","+ xStr +")");
		forwardChainingMap[mummyY][mummyX].mummy = 0;
		return true;
	}
	return false;
}

void Robot::loadKB(char* fileName)
{
	logic.loadKB(fileName);
}
void Robot::loadMap(char* fileName)
{

	map.loadMap(fileName);
	nRow = map.getNRow();
	nCol = map.getNCol();
	forwardChainingMap.resize(nRow);
	for(int i=0;i<nRow;i++)
	{
		forwardChainingMap[i].resize(nCol);
	}

	for(int i=0;i<nRow;i++)
	{
		logic.addSingleExpression(createLogicString("NOT MUMMY",-1,i));
		logic.addSingleExpression(createLogicString("NOT MUMMY",nCol,i));

		logic.addSingleExpression(createLogicString("NOT PIT",-1,i));
		logic.addSingleExpression(createLogicString("NOT PIT",nCol,i));
	}
	for(int i=0;i<nCol;i++)
	{
		logic.addSingleExpression(createLogicString("NOT MUMMY",i,-1));
		logic.addSingleExpression(createLogicString("NOT MUMMY",i,nRow));

		logic.addSingleExpression(createLogicString("NOT PIT",i,-1));
		logic.addSingleExpression(createLogicString("NOT PIT",i,nRow));
	}

}
string Robot::createLogicString(string name,int x,int y)
{
	char buffer[10];
	string xStr(_itoa(x,buffer,10));
	string yStr(_itoa(y,buffer,10));
	return name+"("+xStr+","+yStr+")";
}
bool Robot::checkCell(int x,int y)
{
	forwardChainingMap[y][x].visited = true;
	if(map.containAt(x,y,map.BREEZY))
	{
		logic.addSingleExpression(createLogicString("BREEZY",x,y));
	}
	else
	{
		logic.addSingleExpression(createLogicString("NOT BREEZY",x,y));
	}
	if(map.containAt(x,y,map.STENCH))
	{
		logic.addSingleExpression(createLogicString("STENCH",x,y));
	}
	else
	{
		logic.addSingleExpression(createLogicString("NOT STENCH",x,y));
	}
	if(map.containAt(x,y,map.GOLD))
	{
		logic.addSingleExpression(createLogicString("GOLD",x,y));
		return true;
	}
	return false;
}
void Robot::setConnected(int x,int y)
{
	if(x >0)
	{
		forwardChainingMap[y][x-1].connected = true;
	}
	if(x < nCol-1)
	{
		forwardChainingMap[y][x+1].connected = true;
	}
	if(y >0)
	{
		forwardChainingMap[y-1][x].connected = true;
	}
	if(y < nRow-1)
	{
		forwardChainingMap[y+1][x].connected = true;
	}
}
void Robot::detectCell(int x,int y)
{
	
	if(forwardChainingMap[y][x].mummy == -1)
	{
		if(logic.FOL_FC_Ask(createLogicString("MUMMY",x,y)))
		{
			for(int i=0;i<nRow;i++)
			{
				for(int j=0;j<nCol;j++)
				{
					forwardChainingMap[i][j].mummy = 0;
				}
			}

			forwardChainingMap[y][x].mummy = 1;
			mummyX = x;
			mummyY = y;
			forwardChainingMap[y][x].pit = 0;
		}
		else if(logic.FOL_FC_Ask(createLogicString("NOT MUMMY",x,y)))
		{
			forwardChainingMap[y][x].mummy = 0;
		}
	}
	if(forwardChainingMap[y][x].pit == -1)
	{
		if(logic.FOL_FC_Ask(createLogicString("PIT",x,y)))
		{
			forwardChainingMap[y][x].pit = 1;
			forwardChainingMap[y][x].mummy = 0;
		}
		else if(logic.FOL_FC_Ask(createLogicString("NOT PIT",x,y)))
		{
			forwardChainingMap[y][x].pit = 0;
		}
	}
}
bool Robot::setStartPoint(int x,int y)
{
	turnCost = 0;
	huntingModeOn = false;
	mummyX = -1;
	mummyY = -1;
	if(map.containAt(x,y,map.MUMMY) || map.containAt(x,y,map.PIT))
	{
		return false;
	}
	X=x;
	Y=y;

	direction = RIGHT;
	nArowLeft = 1;
	logic.addSingleExpression(createLogicString("NOT MUMMY",x,y));
	logic.addSingleExpression(createLogicString("NOT PIT",x,y));
	forwardChainingMap[y][x].mummy = 0;
	forwardChainingMap[y][x].pit = 0;
	forwardChainingMap[y][x].connected = true;

	checkCell(x,y);
	setConnected(x,y);
	return true;

}


void add2Queue(NodeState node, vector<NodeState>& priorQueue)
{
	priorQueue.push_back(node);
	
	int j = priorQueue.size()-1;
	while(j >0 && node.cost < priorQueue[j].cost)
	{
		j--;
		priorQueue[j+1] = priorQueue[j];

	}
	priorQueue[j] = node;

}

int Robot::getTurnCost(int currentDir,int targetDir)
{

	int count = 0;
	int tempDir = currentDir;
	while(tempDir != targetDir && count < 3)
	{
		count ++;
		tempDir = toTheLeft(tempDir);
	}
	if(count == 2)
		count = 1;

	return count;
}

void Robot::turnAndGoForward(int targetDir)
{

	int count = 0;
	int tempDir = direction;
	while(tempDir != targetDir && count < 3)
	{
		count ++;
		tempDir = toTheLeft(tempDir);
	}
	if(count == 3)
	{
		turnRight();
		count = 0;
	}
	for(int i = 0;i<count;i++)
	{
		turnLeft();
	}
	forward();

}
void Robot::shootTheMummy()
{
	int dir ;
	if(mummyX  == X)
	{
		if(mummyY > Y)
			dir = DOWN;
		else
			dir = UP;
	}
	if(mummyY  == Y)
	{
		if(mummyX > X)
			dir = RIGHT;
		else
			dir = LEFT;
	}

	int count = 0;
	int tempDir = direction;
	while(tempDir != dir && count < 3)
	{
		count ++;
		tempDir = toTheLeft(tempDir);
	}
	if(count == 3)
	{
		turnRight();
		count = 0;
	}
	for(int i = 0;i<count;i++)
	{
		turnLeft();
	}
	shoot();
	mummyX = -1;
	mummyY = -1;
}
bool Robot::findNewCell()
{
	vector<vector<NodeCellMatrix>> matrix;
	matrix.resize(nRow);
	for(int i=0;i<nRow;i++)
	{
		matrix[i].resize(nCol);
		for(int j=0;j<nCol;j++)
		{
			if(forwardChainingMap[i][j].isOK())
			{
				matrix[i][j].available = true;
			}
		}
	}
	vector<NodeState> priorQueue;
	NodeState node;
	node.set(X,Y,0,direction);
	priorQueue.push_back(node);

	while(priorQueue.size()>0)
	{
		int x = priorQueue[0].x;
		int y = priorQueue[0].y;
		int dir = priorQueue[0].direction;
		if((!forwardChainingMap[y][x].visited && forwardChainingMap[y][x].isOK()) || (huntingModeOn && (x == mummyX || y == mummyY)))
		{
			NodeCellMatrix newNode;
			matrix[Y][X] = newNode;

			int newDir=dir;
			int newX = x;
			int newY = y;
			vector<int> dirList;

			while(newDir != -1)
			{
				dirList.push_back(newDir);
				switch(newDir)
				{
				case LEFT:
				
					newDir = matrix[newY][newX].lastDirectionLeft;
					newX = newX+1;	
					break;
				case RIGHT:
				
					newDir = matrix[newY][newX].lastDirectionRight;
					newX = newX-1;	
					break;					
				case UP:
				
					newDir = matrix[newY][newX].lastDirectionUp;
					newY = newY+1;	
					break;
				case DOWN:
				
					newDir = matrix[newY][newX].lastDirectionDown;
					newY = newY-1;	
					break;
				}
				
			}

			for(int i=dirList.size()-2 ;i>=0;i--)
			{
				turnAndGoForward(dirList[i]);
			}
			if(huntingModeOn)
				shootTheMummy();
			return true;
		}

		int g = priorQueue[0].cost;

		for(int i=1;i<priorQueue.size();i++)
		{
			priorQueue[i-1] = priorQueue[i];
		}
		priorQueue.pop_back();
		int newG = g + getTurnCost(dir, LEFT)+1;
		if(x >0 && matrix[y][x-1].available && matrix[y][x-1].gLeft > newG)
		{
			matrix[y][x-1].gLeft = newG;
			matrix[y][x-1].lastDirectionLeft = dir;
			node.set(x-1,y,newG,LEFT);
			add2Queue(node, priorQueue);
		}
		newG = g + getTurnCost(dir, RIGHT)+1;
		if(x < nCol-1 && matrix[y][x+1].available && matrix[y][x+1].gRight > newG)
		{
			matrix[y][x+1].gRight= newG;
			matrix[y][x+1].lastDirectionRight = dir;
			node.set(x+1,y,newG,RIGHT);
			add2Queue(node, priorQueue);
		}
		newG = g + getTurnCost(dir, UP)+1;
		if(y >0 && matrix[y-1][x].available && matrix[y-1][x].gUp > newG)
		{
			matrix[y-1][x].gUp= newG;
			matrix[y-1][x].lastDirectionUp = dir;
			node.set(x,y-1,newG,UP);
			add2Queue(node, priorQueue);
		}
		newG = g + getTurnCost(dir, DOWN)+1;
		if(y < nRow-1 && matrix[y+1][x].available && matrix[y+1][x].gDown > newG)
		{
			matrix[y+1][x].gDown = newG;
			matrix[y+1][x].lastDirectionDown = dir;
			node.set(x,y+1,newG,DOWN);
			add2Queue(node, priorQueue);
		}
		
	}
	return false;
}
vector<string> Robot::getListAction()
{
	return listAction;
}

int Robot::update()
{
	bool flag=false;
	int count=0;
	for(int i=0;i<nRow;i++)
	{
		for(int j=0;j<nCol;j++)
		{
			if(forwardChainingMap[i][j].connected && !forwardChainingMap[i][j].visited)
			{
				detectCell(j,i);
				if(forwardChainingMap[i][j].mummy == -1 || forwardChainingMap[i][j].pit == -1)
					count++;
			}
		}
	}
	if(!findNewCell())
	{
		if(count==0)
		{
			return 1;
		}
		else if(huntingModeOn && mummyX == -1 && mummyY == -1)
		{
			return 0;
		}
		else
		{
			huntingModeOn=true;
		}
	}

	if(checkCell(X,Y))
	{
		listAction.push_back("GRAB");
		turnCost += 999;
		return 2;
	}
	setConnected(X,Y);

	return -1;
}
int Robot::getCost()
{
	return turnCost;
};